﻿using LxBasic;
using LxCore;
using System;
using System.Numerics;
using System.Windows.Media.Media3D;

namespace LxCFD
{
    public static class VectorMath
    {
        public static Vector3D ConvertFrom(Vector3D v)
        {
            return new Vector3D(v.X, v.Y, v.Z);
        }
        public static (Vector3D u, Vector3D v) FindVerticalVector(Vector3D vector)
        {
            if (vector.LengthSquared == 0) return (new Vector3D(), new Vector3D());
            Vector3D u = (Math.Abs(vector.X) < Math.Abs(vector.Y)) ?
                new Vector3D(0, vector.Z, -vector.Y) :
                new Vector3D(vector.Z, 0, -vector.X);
            u.Normalize();
            Vector3D v = Vector3D.CrossProduct(vector, u);
            v.Normalize();
            return (u, v);
        }
        public static MatrixTransform3D GetTransform(Vector3d x1Transformed, Vector3d translate)
        {
            double len = x1Transformed.Length();
            if (len == 0) { return new MatrixTransform3D(); }
            (Vector3d u, Vector3d v) = Vector3d.FindVerticalVector(x1Transformed);
            return new MatrixTransform3D(
                    CalTransformMatrix(x1Transformed, len * u, len * v, translate));
        }
        public static Matrix3D CalTransformMatrix(in Vector3d x1end, in Vector3d y1end, in Vector3d z1end, in Vector3d translate)
        {
            return new Matrix3D(
                x1end.X, x1end.Y, x1end.Z, 0,
                y1end.X, y1end.Y, y1end.Z, 0,
                z1end.X, z1end.Y, z1end.Z, 0,
                translate.X, translate.Y, translate.Z, 1
                );
        }

        public static Point3D PointRotate(Point3D Old, Vector3D axis_1, Point3D pAxis, double Radian)
        {//右手坐标系下，vector朝向你，逆时针旋转
            double c = Math.Cos(Radian);
            double c1 = 1 - c;
            double s = Math.Sin(Radian);
            double axy = axis_1.X * axis_1.Y;
            double ayz = axis_1.Y * axis_1.Z;
            double axz = axis_1.X * axis_1.Z;
            return new Point3D(
                (axis_1.X * axis_1.X * c1 + c) * (Old.X - pAxis.X) + (axy * c1 - axis_1.Z * s) * (Old.Y - pAxis.Y) + (axz * c1 + axis_1.Y * s) * (Old.Z - pAxis.Z) + pAxis.X,
                (axy * c1 + axis_1.Z * s) * (Old.X - pAxis.X) + (axis_1.Y * axis_1.Y * c1 + c) * (Old.Y - pAxis.Y) + (ayz * c1 - axis_1.X * s) * (Old.Z - pAxis.Z) + pAxis.Y,
                 (axz * c1 - axis_1.Y * s) * (Old.X - pAxis.X) + (ayz * c1 + axis_1.X * s) * (Old.Y - pAxis.Y) + (axis_1.Z * axis_1.Z * c1 + c) * (Old.Z - pAxis.Z) + pAxis.Z
                );
        }
        public static Point3D ProjectPointToPlane(Point3D pointOut, Vector3D planeNormal, Point3D planePoint)
        {
            double t = Vector3D.DotProduct(planeNormal, pointOut - planePoint);
            return pointOut - t / planeNormal.LengthSquared * planeNormal;
        }
        public static Vector3D ProjectVectorToPlane(Vector3D Vector, Vector3D planeNormal)//矢量在平面上的投影
        {
            return Vector - Vector3D.DotProduct(Vector, planeNormal) / planeNormal.LengthSquared * planeNormal;
        }

    }
}
